#include <rov/motor.h>

#include <stdlib.h>
#include <string.h>
#include <unistd.h>
#include <fcntl.h>
#include <termios.h>

#include <iostream>
#include <sstream>


using namespace std;


// Same as unix read(int, void *, size_t)
// except reads until count bytes are read.
// Less than count bytes might be read if
// there is an IO error (e.g. fd gets closed).
// Returns the amount of bytes read.
int readAll(int fd, char * buf, int count)
{
    int n = 0;

    while (n < count)
    {
        int amt = read(fd, &buf[n], count-n);

        // check for EOF (amt = 0) or error (amt < 0)
        if (amt <= 0)
            break;

        n += amt;
    }

    return n;
}


// Returns 1 if the data read from the
// device matches the data in id.
// id should be null terminated.
int checkMatches(int fd, char const * const id)
{
    if (fd < 0)
        return 0;

    int bufLen = strlen(id);
    char * buf = new char[bufLen+1];
    int n = readAll(fd, buf, bufLen);
    int matches = 0;

    if (n == bufLen)
    {
        buf[n] = 0;
        matches = (strcmp(buf, id) == 0);

cerr << "device id: \"" << buf << "\"\n";
    }

    delete buf;

    return matches;
}


struct flock fl;


// Configure a serial connection.
// returns 0 on success
int configSerial(int fd, speed_t baudRate)
{
    struct termios config;

fl.l_type = F_WRLCK;
fl.l_whence = SEEK_SET;
fl.l_start = 0;
fl.l_len = 0;

    int err = fcntl(fd, F_SETLK, &fl);

    if (err)
        return err;

    err = (isatty(fd) != 1);

    if (err)
        return err;

    // get current config
    err = tcgetattr(fd, &config);

    if (err)
        return err;

    // set input baud rate
    err = cfsetispeed(&config, baudRate);

    if (err)
        return err;

    // set output baud rate
    err = cfsetospeed(&config, baudRate);

    if (err)
        return err;

    // apply new config
    err = tcsetattr(fd, TCSAFLUSH, &config);

    return err;
}
